"""
Brief  : tk写的基于B样条的路径生成工具
Author : 傅帅
Date   : 2020.4.16
"""
import os
import json
from math import *
import tkinter as tk
import numpy as np
from shutil import copyfile, rmtree
from tkinter import filedialog
from tkinter import messagebox
from PIL import Image, ImageTk
import matplotlib.pyplot as plt
from BR_Bspline import BR_BSpline

root = tk.Tk
# 图片相关
field = None  # 这个用于存储最后用于画在canvas上的PhotoImage类型的对象，防止其作为局部变量被回收，canvas画img的大坑-_-|.......
field_path = "default.jpg"
field_size = [0,0]
chassis_size = [0,0]
canvas = tk.Canvas
canvas_menu = tk.Menu
field_canvas_id = 0
draggingCP_flag = False
draggingCP_index = 0
# B样条相关
K = 3  # B样条阶数，默认取3
max_acc = 0
max_vel = 0
BSpline = BR_BSpline
ControlPoints = []  # [[x,y], ...] 存储时按canvas内坐标，生成或导入路径时再做转换为实际坐标
path_tbl_sparse = []  # 结构为[[t, x, y, R, a_t, vel, s, time, vel_lim], ...]的稀疏路径表[mm]
path_tbl_fin = []  # 结构为[[vx, vy, x, y, theta(rad)], ...]的最终输出路径表[mm]
path_tbl_config = {}  # 用于存放当前计算路径的各种参数，包括控制点，用于保存和导入路径
# 控件相关
pos_state = tk.StringVar
pos_state_lable = tk.Label
cursor_state = tk.StringVar
cursor_state_lable = tk.Label
field_size_x_entry = tk.Entry
field_size_y_entry = tk.Entry
chassis_size_x_entry = tk.Entry
chassis_size_y_entry = tk.Entry
k_entry = tk.Entry
max_acc_entry = tk.Entry
max_vel_entry = tk.Entry
start_theta_entry = tk.Entry
end_theta_entry = tk.Entry
points_gap_entry = tk.Entry
path_name_entry = tk.Entry
XNegativeCheckVar = tk.IntVar
# 一些配置
CANVAS_WIDTH = 900
CANVAS_HEIGHT = 550

def GUI_Init():
    """GUI布局初始化"""
    global root,field,canvas,canvas_menu,field_canvas_id,pos_state,pos_state_lable,cursor_state,cursor_state_lable,\
           field_size_x_entry,field_size_y_entry,k_entry,max_acc_entry,max_vel_entry,start_theta_entry,end_theta_entry,\
           points_gap_entry,path_name_entry,chassis_size_x_entry,chassis_size_y_entry,XNegativeCheckVar
    # ---------------------------------- 根窗体 ----------------------------------------
    root = tk.Tk()
    root.title('BR_PathGenerator')
    root['width'] = 1000
    root['height'] = 800
    # Esc返回正常模式
    root.bind("<Escape>", OnEsc)
    # ----------------------------- 容器LableFrame -------------------------------------
    panel_frame = tk.LabelFrame(root, text="控制面板", labelanchor="nw")
    panel_frame['width'] = 950
    panel_frame['height'] = 200
    panel_frame.place(x=30, y=590)
    LINE1X = 40
    LINE1Y = 25
    LINE2X = 40
    LINE2Y = 100
    # ---------------------------------- 画布 ----------------------------------------
    # 读入并缩放场地图片
    field_org = Image.open(field_path)  
    field_resized = field_org.resize((CANVAS_WIDTH, CANVAS_HEIGHT), Image.ANTIALIAS)  # 缩放至预设尺寸
    field = ImageTk.PhotoImage(field_resized)
    # 创建画布
    canvas = tk.Canvas(root, bg = "white", width = CANVAS_WIDTH, height = CANVAS_HEIGHT)
    field_canvas_id = canvas.create_image(CANVAS_WIDTH/2, CANVAS_HEIGHT/2, image = field) 
    canvas.place(x=45, y=30)
    # 绑定鼠标事件
    canvas.bind('<Button-1>', onCanvasLeftButtonDown)
    canvas.bind('<B1-Motion>', onCanvasLeftButtonMove)
    canvas.bind('<ButtonRelease-1>', onCanvasLeftButtonUp)
    canvas.bind('<ButtonRelease-3>', onCanvasRightButtonUp)
    # 画布下的菜单
    canvas_menu = tk.Menu(canvas, tearoff=0)
    canvas_menu.add_command(label='AddControlPoint', command=AddCpBtnCb)
    canvas_menu.add_command(label='DelControlPoint', command=DelCpBtnCb)
    canvas_menu.add_command(label='BackToNormal', command=Back2NormalBtnCb)
    canvas_menu.add_separator()
    canvas_menu.add_command(label='ClearAll', command=ClearAllBtnCb)
    # ---------------------------------- 标签 ----------------------------------------
    # 动态标签
    pos_state = tk.StringVar(root, "请输入场地尺寸")
    pos_state_lable = tk.Label(root, textvariable = pos_state, font='SimHei -15', fg='red')
    pos_state_lable.place(x=45, y=5)
    cursor_state = tk.StringVar(root, "Normal")
    cursor_state_lable = tk.Label(root, textvariable = cursor_state, font='SimHei -15', fg='green')
    cursor_state_lable.place(x=900, y=5)
    # 静态标签
    canvas_title_lable = tk.Label(root, text="场地/轨迹图", font="SimHei -15", fg='black')
    canvas_title_lable.place(x=450, y=5)
    cursor_state_pre_lable = tk.Label(root, text="CursorState:", font="SimHei -15")
    cursor_state_pre_lable.place(x=800, y=5)
    field_sizex_lable = tk.Label(panel_frame, text="场地尺寸X(mm):")
    field_sizex_lable.place(x=LINE1X+150, y=LINE1Y-25)
    field_sizey_lable = tk.Label(panel_frame, text="Y(mm):")
    field_sizey_lable.place(x=LINE1X+330, y=LINE1Y-25)
    tk.Label(panel_frame, text="车体尺寸长(mm):") .place(x=LINE1X+150, y=LINE1Y+10)
    tk.Label(panel_frame, text="宽(mm):") .place(x=LINE1X+330, y=LINE1Y+10)
    k_entry_lable = tk.Label(panel_frame, text="B样条曲线阶数:")
    k_entry_lable.place(x=LINE1X+640, y=LINE1Y)
    max_a_lable = tk.Label(panel_frame, text="最大a(m/s^2):")
    max_a_lable.place(x=LINE2X, y=LINE2Y-35)
    max_a_lable = tk.Label(panel_frame, text="最大v(m/s):")
    max_a_lable.place(x=LINE2X+155, y=LINE2Y-35)
    theta_lable = tk.Label(panel_frame, text="起止角度(°):")
    theta_lable.place(x=LINE2X, y=LINE2Y)
    to_lable = tk.Label(panel_frame, text="to")
    to_lable.place(x=LINE2X+125, y=LINE2Y)
    points_gap_lable = tk.Label(panel_frame, text="点间距(m):")
    points_gap_lable.place(x=LINE2X, y=LINE2Y+35)
    # ---------------------------------- 按钮 ----------------------------------------
    field_btn = tk.Button(panel_frame, text="导入场地图片", command=fieldBtnCb)
    field_btn.place(x=LINE1X, y=LINE1Y-4)
    field_size_btn = tk.Button(panel_frame, text="更新场地尺寸", command=fieldSizeBtnCb)
    field_size_btn.place(x=LINE1X+470, y=LINE1Y-29)
    chassis_size_btn = tk.Button(panel_frame, text="更新底盘尺寸", command=chassisSizeBtnCb)
    chassis_size_btn.place(x=LINE1X+470, y=LINE1Y+4)
    k_btn = tk.Button(panel_frame, text="更新阶数", command=kBtnCb)
    k_btn.place(x=LINE1X+790, y=LINE1Y-4)
    calculate_btn = tk.Button(panel_frame, text="计算路径", command=calBtnCb)
    calculate_btn.place(x=LINE2X+230, y=LINE2Y+30)
    draw_vel_btn = tk.Button(panel_frame, text="速度图像", command=drawVelBtn)  # v-t, v-s
    draw_vel_btn.place(x=LINE2X+350, y=LINE2Y-4)
    draw_acc_btn = tk.Button(panel_frame, text="加速度图像", command=drawAccBtn)  # a-t, a-s
    draw_acc_btn.place(x=LINE2X+450, y=LINE2Y-4)
    save_path_btn = tk.Button(panel_frame, text="保存路径", command=savePathBtn)
    save_path_btn.place(x=LINE2X+700, y=LINE2Y-4)
    load_path_btn = tk.Button(panel_frame, text="导入路径", command=loadPathBtn)
    load_path_btn.place(x=LINE2X+800, y=LINE2Y-4)
    # ---------------------------------- 输入框 ----------------------------------------
    field_size_x_entry = tk.Entry(panel_frame, width="10")
    field_size_x_entry.place(x=LINE1X+250, y=LINE1Y-23)
    field_size_y_entry = tk.Entry(panel_frame, width="10")
    field_size_y_entry.place(x=LINE1X+380, y=LINE1Y-23)
    chassis_size_y_entry = tk.Entry(panel_frame, width="10")
    chassis_size_y_entry.place(x=LINE1X+250, y=LINE1Y+7)
    chassis_size_x_entry = tk.Entry(panel_frame, width="10")
    chassis_size_x_entry.place(x=LINE1X+380, y=LINE1Y+7)
    k_entry = tk.Entry(panel_frame, width='5')
    k_entry.place(x=LINE1X+735, y=LINE1Y+2)
    k_entry.insert(0, 3)
    max_acc_entry = tk.Entry(panel_frame, width='8')
    max_acc_entry.place(x=LINE2X+90, y=LINE2Y+2-35)
    max_vel_entry = tk.Entry(panel_frame, width='8')
    max_vel_entry.place(x=LINE2X+225, y=LINE2Y+2-35)
    start_theta_entry = tk.Entry(panel_frame, width='6')
    start_theta_entry.place(x=LINE2X+75, y=LINE2Y+2)
    start_theta_entry.insert(0, 0.0)
    end_theta_entry = tk.Entry(panel_frame, width='6')
    end_theta_entry.place(x=LINE2X+145, y=LINE2Y+2)
    end_theta_entry.insert(0, 0.0)
    points_gap_entry = tk.Entry(panel_frame, width='6')
    points_gap_entry.place(x=LINE2X+65, y=LINE2Y+2+35)
    points_gap_entry.insert(0, 0.1)
    path_name_entry = tk.Entry(panel_frame, width='12')
    path_name_entry.place(x=LINE2X+600, y=LINE2Y+2)
    XNegativeCheckVar = tk.IntVar()
    tk.Checkbutton(panel_frame, text = "X方向取负号", variable = XNegativeCheckVar, onvalue=-1, offvalue=1).place(x=LINE2X+600, y=LINE2Y+30)
    # ---------------------------------- 启动 ----------------------------------------
    root.protocol("WM_DELETE_WINDOW", OnClose)  # 注册关闭事件
    load_config('config.json')  # 导入保存的配置
    root.mainloop()

# ---------------------------------- 画布相关回调 ----------------------------------------
CP_BoxSize = 10
def _draw_CP(x, y):
    return canvas.create_rectangle(x - CP_BoxSize/2, y - CP_BoxSize/2,\
                                   x + CP_BoxSize/2, y + CP_BoxSize/2, fill="#FF0000")

def _draw_BSpinle():
    t_list = np.linspace(0,1,200)
    x_list, y_list = BSpline(t_list)
    _draw_circles(canvas, x_list, y_list, 1)

def _draw_circles(canvas, x_list, y_list, r):
    for i in range(len(x_list)):
        canvas.create_oval(x_list[i]-r, y_list[i]-r, x_list[i]+r, y_list[i]+r, fill='blue', outline="blue")
    
def _draw_chassis_size():
    # 在首末点处画底盘尺寸矩形
    rect_size_pix = [chassis_size[0] * CANVAS_WIDTH / field_size[0], chassis_size[1] * CANVAS_HEIGHT / field_size[1]]
    for i in [0, -1]:  # 首末点
        p = ControlPoints[i]
        for j in [0, 1]:  # 底盘长和宽
            canvas.create_rectangle(p[0] - rect_size_pix[j]/2, p[1] - rect_size_pix[j]/2,\
                                           p[0] + rect_size_pix[j]/2, p[1] + rect_size_pix[j]/2, outline="blue")

def update_canvas():
    """清空画布，然后根据field、控制点、BSpline三个全局变量重新渲染画布"""
    global field_canvas_id
    if len(ControlPoints)==0: return
    canvas.delete("all")
    field_canvas_id = canvas.create_image(CANVAS_WIDTH/2, CANVAS_HEIGHT/2, image = field)
    _draw_BSpinle()
    for cp in ControlPoints:
        _draw_CP(cp[0], cp[1])
    _draw_chassis_size()
    
def onCanvasLeftButtonDown(event):
    state = cursor_state.get()
    if state == "Normal":  # 若在点上，进入拖动状态
        global draggingCP_flag, draggingCP_index
        for index, cp in enumerate(ControlPoints):  # 若在cpBox范围内
            if event.x > cp[0] - CP_BoxSize/2 and event.x < cp[0] + CP_BoxSize/2 and \
               event.y > cp[1] - CP_BoxSize/2 and event.y < cp[1] + CP_BoxSize/2:
                draggingCP_flag = True
                draggingCP_index = index
    elif state == "AddCP":
        pass
    elif state == "DelCP":
        pass

def onCanvasLeftButtonMove(event):
    """ 按下左键移动，若图片尺寸已确定，则显示x,y在图中的实际坐标
        以及若有控制点处于被拖动状态，则不断更新控制点、BSpline和canvas
    """
    global ControlPoints, BSpline
    if field_size[0] != 0 and field_size[1] != 0:
        cur_x = event.x * field_size[0] / CANVAS_WIDTH
        cur_y = event.y * field_size[1] / CANVAS_HEIGHT
        if cur_x > field_size[0] or cur_y > field_size[1] or cur_x < 0 or cur_y < 0: return
        pos_state_lable['fg'] = 'green'
        pos_state.set("X:{:.2f} Y:{:.2f}".format(cur_x, cur_y))  
    if draggingCP_flag == True:
        ControlPoints[draggingCP_index][0] = event.x  # 更新控制点
        ControlPoints[draggingCP_index][1] = event.y
        BSpline = BR_BSpline(K, ControlPoints)  # 更新BSpline
        update_canvas()  # 更新画布

def onCanvasLeftButtonUp(event):
    global BSpline, draggingCP_flag
    state = cursor_state.get()
    if state == "Normal":  # 若在点上，离开拖动状态
        draggingCP_flag = False
    elif state == "AddCP":  # 按一定逻辑画点并加入CP队列，更新BSpline
        for cp in ControlPoints:  # 若在CPBox内已有点，退出，防止重复添加
            if event.x > cp[0] - CP_BoxSize/2 and event.x < cp[0] + CP_BoxSize/2 and \
               event.y > cp[1] - CP_BoxSize/2 and event.y < cp[1] + CP_BoxSize/2:
                return
        if len(ControlPoints) <= 1:  # 前两个点，默认为首末点
            _draw_CP(event.x, event.y)
            ControlPoints.append([event.x,event.y])
        elif len(ControlPoints) == 2:  # 第三个点，默认为中间
            _draw_CP(event.x, event.y)
            ControlPoints.insert(1, [event.x,event.y])
        else:  # 后续点，依照前后向量点积判断插在哪个位置(权宜之计，可先插，确定下标后再拖走)
            for i in range(len(ControlPoints)-1):
                vector1 = (ControlPoints[i][0] - event.x, ControlPoints[i][1] - event.y)
                vector2 = (ControlPoints[i+1][0] - event.x, ControlPoints[i+1][1] - event.y)
                if vector1[0]*vector2[0] + vector1[1]*vector2[1] <= 0:  # 钝角或直角
                    _draw_CP(event.x, event.y)
                    ControlPoints.insert(i+1, [event.x, event.y])
                    break
        if len(ControlPoints) >= 2 and len(ControlPoints) >= K-1:  # 控制点数目大于等于2且大于等于K-1，才可创建BSpline
            BSpline = BR_BSpline(K, ControlPoints)  # 取前两行，更新BSpline
            update_canvas()  # 重新渲染canvas
    elif state == "DelCP":  # 遍历CP队列并删除，更新BSpline
        for cp in ControlPoints:  # 若在cpBox范围内
            if event.x > cp[0] - CP_BoxSize/2 and event.x < cp[0] + CP_BoxSize/2 and \
               event.y > cp[1] - CP_BoxSize/2 and event.y < cp[1] + CP_BoxSize/2:
                ControlPoints.remove(cp)
                if len(ControlPoints) >= 2 and len(ControlPoints) >= K-1:  # 控制点数目大于等于2且大于等于K-1，才可创建BSpline
                    BSpline = BR_BSpline(K, ControlPoints)  # 更新BSpline
                    update_canvas()  # 更新画布
                else:  # 不更新BSpline的情况下更新画布
                    global field_canvas_id
                    canvas.delete("all")
                    field_canvas_id = canvas.create_image(CANVAS_WIDTH/2, CANVAS_HEIGHT/2, image = field)
                    for cp in ControlPoints:
                        _draw_CP(cp[0], cp[1])

def onCanvasRightButtonUp(event):
    canvas_menu.post(event.x_root, event.y_root) 

def OnEsc(event):
    Back2NormalBtnCb()

# -------------------------------- 右键菜单回调 -------------------------------------------
def AddCpBtnCb():
    """进入增加控制点模式"""
    cursor_state.set("AddCP")
    canvas["cursor"] = "cross"
    cursor_state_lable['fg'] = 'blue'
def DelCpBtnCb():
    cursor_state.set("DelCP")
    canvas["cursor"] = "X_cursor"
    cursor_state_lable['fg'] = 'red'
def Back2NormalBtnCb():
    cursor_state.set("Normal")
    canvas["cursor"] = ""
    cursor_state_lable['fg'] = 'green'
def ClearAllBtnCb():
    global field_canvas_id, ControlPoints
    Back2NormalBtnCb()
    ControlPoints = []
    canvas.delete("all")
    field_canvas_id = canvas.create_image(CANVAS_WIDTH/2, CANVAS_HEIGHT/2, image = field)


# ---------------------------------- 控制面板相关回调 ----------------------------------------
def fieldBtnCb():
    """导入场地图，更新至canvas设定尺寸"""
    global field,field_path,field_canvas_id
    input_path = tk.filedialog.askopenfilename(title='导入场地图片', filetypes=[('场地图片', '*.jpg *.png')])
    # 读入并缩放场地图片
    if input_path != "": field_path = input_path
    field_org = Image.open(field_path)  
    field_resized = field_org.resize((CANVAS_WIDTH, CANVAS_HEIGHT), Image.ANTIALIAS)  # 缩放至预设尺寸
    field = ImageTk.PhotoImage(field_resized)
    canvas.delete(field_canvas_id)
    field_canvas_id = canvas.create_image(CANVAS_WIDTH/2, CANVAS_HEIGHT/2, image = field)

def fieldSizeBtnCb():
    """设定场地尺寸"""
    global field_size
    if field_path == 'default.jpg': tk.messagebox.showinfo("提示","请先选择场地图片");return
    if field_size_x_entry.get() == "" or field_size_y_entry.get() == "": tk.messagebox.showinfo("提示","请输入场地尺寸");return
    field_size = [float(field_size_x_entry.get()), float(field_size_y_entry.get())]
    pos_state_lable['fg'] = 'green'
    pos_state.set("按住鼠标左键可查看坐标哦")
    update_canvas()

def chassisSizeBtnCb():
    """设定底盘尺寸"""
    global chassis_size
    if chassis_size_x_entry.get() == "" or chassis_size_y_entry.get() == "": tk.messagebox.showinfo("提示","请输入底盘尺寸");return
    chassis_size = [float(chassis_size_x_entry.get()), float(chassis_size_y_entry.get())]
    update_canvas()

def kBtnCb():
    """更新阶数"""
    global K, BSpline
    K = int(k_entry.get())
    if len(ControlPoints) >= K-1:  # 控制点数目大于等于2且大于等于K-1，才可创建BSpline
        BSpline = BR_BSpline(K, ControlPoints)  # 更新BSpline
        update_canvas()  # 更新画布
    else: tk.messagebox.showinfo("提示","控制点数目太少，应大于等于K-1");return

def calBtnCb():
    """
    对BR_BSpline计算生成的稠密路径表进行处理，并整理成需要的各种格式
    """
    global BSpline, path_tbl_sparse, path_tbl_fin, path_tbl_config
    # 错误情况判断
    if field_size == [0,0]: tk.messagebox.showinfo("提示","请先确定场地尺寸");return
    if chassis_size == [0,0]: tk.messagebox.showinfo("提示","请先确定底盘尺寸");return
    if max_acc_entry.get() == "" or max_vel_entry.get() == "": tk.messagebox.showinfo("提示","清设置最大加速度/速度");return
    if start_theta_entry.get() == "" or end_theta_entry.get() == "": tk.messagebox.showinfo("提示","请设置起始角度");return
    if points_gap_entry.get() == "": tk.messagebox.showinfo("提示","请设置点间距");return
    if len(ControlPoints) < K-1: tk.messagebox.showinfo("提示","控制点数目太少，应大于等于K-1");return
    if len(ControlPoints) < 3: tk.messagebox.showinfo("提示","这是B样条曲线路径生成器，直线部分还没写，至少来三个控制点吧XD");return
    if K < 2: tk.messagebox.showinfo("提示","这是B样条曲线路径生成器，直线部分还没写，至少阶数大于1吧XD");return
    # 整理得到实际场地尺寸作为控制点的BSpline
    CP = np.array(ControlPoints) * 1.0
    CP[:,0] *= (field_size[0]/CANVAS_WIDTH)
    CP[:,1] *= (field_size[1]/CANVAS_HEIGHT)
    BSpline = BR_BSpline(K, CP)  # 更新BSpline
    path_tbl_org = BSpline.path_generate_dense(float(max_acc_entry.get()), float(max_vel_entry.get()))
    # 依照给定的点间距，对稠密的path_tbl_org进行稀疏化
    gap = float(points_gap_entry.get())*1000.0
    path_tbl_sparse = []  # 清空
    path_tbl_sparse.append(path_tbl_org[0])  # 加入第一个点
    for point in path_tbl_org:
        if point[6] - path_tbl_sparse[-1][6] >= gap:
            path_tbl_sparse.append(point)
    path_tbl_sparse.pop()  # 这里推出一个，为保证最后一个点和倒第二个点的gap在[gap,2gap]间，而非[0,gap]间
    path_tbl_sparse.append(path_tbl_org[-1])  # 加入最后一个点
    # 角度线性插值
    theta_tbl = np.linspace(float(start_theta_entry.get()), float(end_theta_entry.get()), len(path_tbl_sparse))
    # 转换回canvas尺寸，将稀疏路径点画在图上，并每隔一段距离画出底盘矩形
    BSpline = BR_BSpline(K, ControlPoints)  # 将BSpline转回canvas尺寸，后面用不到原尺寸的了
    update_canvas()
    R = 2
    x_list = np.array(path_tbl_sparse)[:,1] * CANVAS_WIDTH / field_size[0]
    y_list = np.array(path_tbl_sparse)[:,2] * CANVAS_HEIGHT / field_size[1]
    rect_size_pix = [chassis_size[0] * CANVAS_WIDTH / field_size[0], chassis_size[1] * CANVAS_HEIGHT / field_size[1]]  # 底盘矩形
    rect_r = int(sqrt(rect_size_pix[0]**2 + rect_size_pix[1]**2)/2)  # 矩形半径
    theta0 = atan2(chassis_size[1], chassis_size[0])
    _gap = 1000.0  # 画底盘矩形的间隔[mm]
    _sum = 0.0
    for i in range(len(path_tbl_sparse)):  # 画稀疏路径点和底盘矩形
        canvas.create_oval(x_list[i]-R, y_list[i]-R, x_list[i]+R, y_list[i]+R, fill='red', outline="red")
        if path_tbl_sparse[i][-3] >= _sum or i == len(path_tbl_sparse)-1:  # 到达间隔或最后一点，画旋转后的矩形
            _sum += _gap
            theta = -theta_tbl[i]/180.0*pi
            center = [x_list[i], y_list[i]]
            polygon_points = [
                center[0]+rect_r*cos(theta0+theta), center[1]+rect_r*sin(theta0+theta),
                center[0]+rect_r*cos(pi-theta0+theta), center[1]+rect_r*sin(pi-theta0+theta),
                center[0]+rect_r*cos(pi+theta0+theta), center[1]+rect_r*sin(pi+theta0+theta),
                center[0]+rect_r*cos(-theta0+theta), center[1]+rect_r*sin(-theta0+theta),
            ]
            canvas.create_polygon(polygon_points, outline='#88FFFF', fill='')

    # 对稀疏路径表进行整理，加上输入的起始角度线性插值，得到最终用于输出的path_tbl_fin
    path_tbl_fin = []
    for index, p in enumerate(path_tbl_sparse):
        # 计算该点到下一点的速度xy分量
        if index != 0 and index != len(path_tbl_sparse) - 1:  # 正常情况计算
            p1 = p
            p2 = path_tbl_sparse[index+1]
            delta_x = p2[1] - p1[1]
            delta_y = p2[2] - p1[2]
            distance = sqrt(delta_x**2 + delta_y**2)
            vel_x = p[5] * delta_x/distance
            vel_y = p[5] * delta_y/distance
        else:  # 第一点和最后一点速度直接给0
            vel_x = 0
            vel_y = 0
        # [vx, vy, x, y, theta]
        path_tbl_fin.append([vel_x, vel_y, p[1], p[2], theta_tbl[index]/180.0*pi])
    # 更新一次path_tbl_config
    path_tbl_config = { "field_path"    : field_path,
                        "field_size"    : (float(field_size_x_entry.get()),float(field_size_y_entry.get())),
                        "chassis_size"  : (float(chassis_size_x_entry.get()),float(chassis_size_y_entry.get())),
                        "K"             : int(k_entry.get()),
                        "max_acc"       : float(max_acc_entry.get()),
                        "max_vel"       : float(max_vel_entry.get()),
                        "start_theta"   : float(start_theta_entry.get()),
                        "end_theta"     : float(end_theta_entry.get()),
                        "points_gap"    : float(points_gap_entry.get()),
                     }
    path_tbl_config["ControlPoints"] = ControlPoints

def drawVelBtn():
    """画V-T V-S图"""
    # 错误情况判断
    if path_tbl_sparse == []: tk.messagebox.showinfo("提示","请先计算路径，才能画图");return
    plt.figure("速度图像", figsize=(12,6))
    plt.subplot("121")
    plt.title("V-T图", fontsize="25")
    plt.xlabel("Time(s)", fontsize="15")
    plt.xticks(fontsize="15")
    plt.ylabel("Vel(m/s)", fontsize="15")
    plt.yticks(fontsize="15")
    plt.ylim(0, max_vel*1.2)
    plt.plot(np.array(path_tbl_sparse)[:,7], np.array(path_tbl_sparse)[:,5]/1000.0, 'b')
    plt.subplot("122")
    plt.title("V-S图", fontsize="25")
    plt.xlabel("路程S(m)", fontsize="15")
    plt.xticks(fontsize="15")
    plt.ylabel("Vel(m/s)", fontsize="15")
    plt.yticks(fontsize="15")
    plt.ylim(0, max_vel*1.2)
    plt.plot(np.array(path_tbl_sparse)[:,6]/1000.0, np.array(path_tbl_sparse)[:,5]/1000.0, 'r')
    plt.show()

def drawAccBtn():
    """画A-T A-S图"""
    # 错误情况判断
    if path_tbl_sparse == []: tk.messagebox.showinfo("提示","请先计算路径，才能画图");return
    plt.figure("加速度图像(该点可提供最大切向加速度)", figsize=(12,6))
    plt.subplot("121")
    plt.title("A-T图", fontsize="25")
    plt.xlabel("Time(s)", fontsize="15")
    plt.xticks(fontsize="15")
    plt.ylabel("Acc(m/s^2)", fontsize="15")
    plt.yticks(fontsize="15")
    plt.ylim(0, max_acc*1.2)
    plt.plot(np.array(path_tbl_sparse)[:,7], np.array(path_tbl_sparse)[:,4]/1000.0, 'b')
    plt.subplot("122")
    plt.title("A-S图", fontsize="25")
    plt.xlabel("路程S(m)", fontsize="15")
    plt.xticks(fontsize="15")
    plt.ylabel("Acc(m/s^2)", fontsize="15")
    plt.yticks(fontsize="15")
    plt.ylim(0, max_acc*1.2)
    plt.plot(np.array(path_tbl_sparse)[:,6]/1000.0, np.array(path_tbl_sparse)[:,4]/1000.0, 'r')
    plt.show()

def savePathBtn():
    """保存路径对应场地图、路径参数、路径实体至同名文件夹下"""
    # 错误情况判断
    if path_tbl_config == [] or path_tbl_fin == []: tk.messagebox.showinfo("提示","请先计算路径，才能保存");return
    if not os.path.isdir('paths'): os.mkdir('paths')  # 若没有paths文件夹，创建之
    path_name = path_name_entry.get()
    if path_name != '':
        file_path = 'paths/' + path_name
        rewrite = False
        if os.path.isdir(file_path):
            if tk.messagebox.askquestion("提示","该路径文件夹已存在，是否要覆盖？"):
                rewrite = True
            else: return
        if rewrite == False:
            os.mkdir(file_path)
        root = file_path + '/'
        # 将路径图片存入文件夹以便于分享
        extern_name = os.path.splitext(path_tbl_config['field_path'])[1]
        if rewrite:  # 若为覆盖，需判断一下是否场地图片是否需要更换
            if path_tbl_config['field_path'] == root+path_name+'_field'+extern_name:
                pass  # 若当前图和目标图相同，啥也不用干
            else:  # 若当前图和目标图不同，删掉目标图，再拷贝
                os.remove(root+path_name+'_field'+extern_name)
                copyfile(path_tbl_config['field_path'], root+path_name+'_field'+extern_name)
        else:  # 不是覆盖直接拷贝就成
                copyfile(path_tbl_config['field_path'], root+path_name+'_field'+extern_name)
        path_tbl_config['field_path'] = root+path_name+'_field'+extern_name  # 将图片路径改成相对路径
        # 写入path_name_config.json用于保存路径参数
        with open(root+path_name + "_config.json", 'w') as f:
            json.dump(path_tbl_config, f)
        # 写入可直接贴到keil中的文件
        with open(root+path_name + ".txt", 'w') as f:
            f.writelines("const float "+path_name+"[][5] = {\n")
            for point in path_tbl_fin:
                output_format = "{{{:>11.3f}f,{:>11.3f}f,{:>11.3f}f,{:>11.3f}f,{:>8.3f}f}},\n"
                line_text = output_format.format(XNegativeCheckVar.get()*point[0],point[1],XNegativeCheckVar.get()*point[2],point[3],point[4])
                f.writelines(line_text)
            f.writelines("};")
            tk.messagebox.showinfo("提示","保存成功！")
    else:
        tk.messagebox.showinfo("提示","请输入路径名")

def loadPathBtn():
    global ControlPoints, BSpline
    input_path = tk.filedialog.askopenfilename(title='导入路径', filetypes=[('xxx_config', '*.json')])
    if input_path != "":
        load_config(input_path)
        with open(input_path, 'r') as f:
            config = json.load(f)
            ControlPoints = config["ControlPoints"]
        BSpline = BR_BSpline(K, ControlPoints)  # 更新BSpline
        update_canvas()
        calBtnCb()  # 执行一次路径计算

def OnClose():
    if tk.messagebox.askokcancel("退出", "确定要退出?"):
        # 保存配置
        try:
            config = {  "field_path"    : field_path,
                        "field_size"    : (float(field_size_x_entry.get()),float(field_size_y_entry.get())),
                        "chassis_size"  : (float(chassis_size_x_entry.get()),float(chassis_size_y_entry.get())),
                        "K"             : int(k_entry.get()),
                        "max_acc"       : float(max_acc_entry.get()),
                        "max_vel"       : float(max_vel_entry.get()),
                        "start_theta"   : float(start_theta_entry.get()),
                        "end_theta"     : float(end_theta_entry.get()),
                        "points_gap"    : float(points_gap_entry.get()),
                     }
            with open('config.json', 'w') as f:
                json.dump(config, f)
        except ValueError:
            tk.messagebox.showinfo("提示","所有配置都已输入时,\r才会保存配置")
        root.destroy()

def load_config(config_file):
    global field,field_path,field_size,chassis_size,field_canvas_id,K,max_acc,max_vel
    try:
        with open(config_file, 'r') as f:
            config = json.load(f)
    except FileNotFoundError:
        config = False
    if config:  # 若配置文件存在，修改一些控件状态，否则保持初始化时默认状态
        # 场地图
        if os.path.isfile(config["field_path"]):  # 若场地文件存在，更新之
            field_path = config["field_path"]
            field_org = Image.open(field_path)  
            field_resized = field_org.resize((CANVAS_WIDTH, CANVAS_HEIGHT), Image.ANTIALIAS)  # 缩放至预设尺寸
            field = ImageTk.PhotoImage(field_resized)
            canvas.delete(field_canvas_id)
            field_canvas_id = canvas.create_image(CANVAS_WIDTH/2, CANVAS_HEIGHT/2, image = field)
        # 尺寸
        field_size = config["field_size"]
        pos_state_lable['fg'] = 'green'
        pos_state.set("X:0 Y:0")  
        field_size_x_entry.delete(0,10)
        field_size_y_entry.delete(0,10)
        field_size_x_entry.insert(0,field_size[0])
        field_size_y_entry.insert(0,field_size[1])
        chassis_size = config["chassis_size"]
        chassis_size_x_entry.delete(0,10)
        chassis_size_y_entry.delete(0,10)
        chassis_size_x_entry.insert(0,chassis_size[0])
        chassis_size_y_entry.insert(0,chassis_size[1])
        # 阶次
        K = config["K"]
        k_entry.delete(0,5)
        k_entry.insert(0,K)
        # 加速度
        max_acc = config["max_acc"]
        max_acc_entry.delete(0,8)
        max_acc_entry.insert(0,max_acc)
        # 速度
        max_vel = config["max_vel"]
        max_vel_entry.delete(0,8)
        max_vel_entry.insert(0,max_vel)
        # 其他
        start_theta_entry.delete(0,6)
        start_theta_entry.insert(0, config["start_theta"])
        end_theta_entry.delete(0,6)
        end_theta_entry.insert(0, config["end_theta"])
        points_gap_entry.delete(0,6)
        points_gap_entry.insert(0, config["points_gap"])

if __name__ == "__main__":
    GUI_Init()
